home *** CD-ROM | disk | FTP | other *** search
/ Aminet 2 / Aminet AMIGA CDROM (1994)(Walnut Creek)[Feb 1994][W.O. 44790-1].iso / Aminet / util / misc / Fudgit233.lha / Source / src / svdfit.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-12-14  |  8.3 KB  |  323 lines
  1. #include <math.h>
  2. #include <stdio.h>
  3. #ifndef NOUNISTD_H
  4. #include <unistd.h>
  5. #endif
  6. #include "dalloca.h"
  7. #include "head.h"
  8.  
  9. #define TOL 1.0e-20
  10.  
  11. #define ERRR (-1)
  12. #ifndef NULL
  13. #define NULL 0
  14. #endif
  15.  
  16. static int svdcmp(double **a, int m, int n, double *w, double **v);
  17. static int svbksb(double **u, double *w, double **v, int m, int n, double *b, double *x);
  18.  
  19. int Ft_svdfit(double **coef, double *y, double *sig, int ndata, double *a, int ma, double **v, double *w, double *chisq)
  20. {
  21.     int j,i;
  22.     double wmax,tmp,thresh,sum,**u,*b;
  23.  
  24.     ADVECTOR(b, 1, ndata, "svdfit", Ft_catcher);
  25.     ADMATRIX(u, 1, ndata, 1, ma, "svdfit", Ft_catcher);
  26.  
  27.     for (i=1;i<=ndata;i++) {
  28.         tmp=1.0/sig[i];
  29.         for (j=1;j<=ma;j++) u[i][j]=coef[j][i]*tmp;
  30.         b[i]=y[i]*tmp;
  31.     }
  32.     if (svdcmp(u,ndata,ma,w,v) == ERRR) {
  33.         Ft_catcher(ERRR);
  34.     }
  35.     wmax=0.0;
  36.     for (j=1;j<=ma;j++)
  37.         if (w[j] > wmax) wmax=w[j];
  38.     thresh=TOL*wmax;
  39.     for (j=1;j<=ma;j++)
  40.         if (w[j] < thresh) w[j]=0.0;
  41.     if (svbksb(u,w,v,ndata,ma,b,a) == ERRR) {
  42.         Ft_catcher(ERRR);
  43.     }
  44.     *chisq=0.0;
  45.     for (i=1;i<=ndata;i++) {
  46.         for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*coef[j][i];
  47.         *chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
  48.     }
  49.     return(1);
  50. }
  51.  
  52. #undef TOL
  53.  
  54. static double at,bt,ct;
  55. #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? (ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)): 0.0))
  56.  
  57. static double maxarg1,maxarg2;
  58. #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
  59. #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
  60.  
  61. static int svdcmp(double **a, int m, int n, double *w, double **v)
  62. {
  63.     int flag, i, its, j, jj, k, l, nm;
  64.     double c, f, h, s, x, y, z;
  65.     double anorm=0.0, g=0.0;
  66.     double scale=0.0;
  67.     double *rv1;
  68.  
  69.     if (m < n) {
  70.         fprintf(stderr, "svdcmp: You must augment A with extra zero rows.");
  71.         return(ERRR);
  72.     }
  73.     ADVECTOR(rv1, 1, n, "svdcmp", return);
  74.  
  75.     l = nm = 0;
  76.     for (i=1;i<=n;i++) {
  77.         l = i+1;
  78.         rv1[i] = scale*g;
  79.         g = s = scale = 0.0;
  80.         if (i <= m) {
  81.             for (k=i;k<=m;k++)
  82.                 scale += fabs(a[k][i]);
  83.             if (scale) {
  84.                 for (k=i;k<=m;k++) {
  85.                     a[k][i] /= scale;
  86.                     s += a[k][i]*a[k][i];
  87.                 }
  88.                 f=a[i][i];
  89.                 g = -SIGN(sqrt(s),f);
  90.                 h = f*g-s;
  91.                 a[i][i] = f-g;
  92.                 if (i != n) {
  93.                     for (j=l;j<=n;j++) {
  94.                         for (s=0.0,k=i;k<=m;k++)
  95.                             s += a[k][i]*a[k][j];
  96.                         f = s/h;
  97.                         for (k=i;k<=m;k++)
  98.                             a[k][j] += f*a[k][i];
  99.                     }
  100.                 }
  101.                 for (k=i;k<=m;k++)
  102.                     a[k][i] *= scale;
  103.             }
  104.         }
  105.         w[i] = scale*g;
  106.         g = s = scale = 0.0;
  107.         if (i <= m && i != n) {
  108.             for (k=l;k<=n;k++)
  109.                 scale += fabs(a[i][k]);
  110.             if (scale) {
  111.                 for (k=l;k<=n;k++) {
  112.                     a[i][k] /= scale;
  113.                     s += a[i][k]*a[i][k];
  114.                 }
  115.                 f = a[i][l];
  116.                 g = -SIGN(sqrt(s),f);
  117.                 h = f*g-s;
  118.                 a[i][l] = f-g;
  119.                 for (k=l;k<=n;k++)
  120.                     rv1[k]=a[i][k]/h;
  121.                 if (i != m) {
  122.                     for (j=l;j<=m;j++) {
  123.                         for (s=0.0,k=l;k<=n;k++)
  124.                             s += a[j][k]*a[i][k];
  125.                         for (k=l;k<=n;k++)
  126.                             a[j][k] += s*rv1[k];
  127.                     }
  128.                 }
  129.                 for (k=l;k<=n;k++)
  130.                     a[i][k] *= scale;
  131.             }
  132.         }
  133.         anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
  134.     }
  135.     for (i=n;i>=1;i--) {
  136.         if (i < n) {
  137.             if (g) {
  138.                 for (j=l;j<=n;j++)
  139.                     v[j][i] = (a[i][j]/a[i][l])/g;
  140.                 for (j=l;j<=n;j++) {
  141.                     for (s=0.0,k=l;k<=n;k++)
  142.                         s += a[i][k]*v[k][j];
  143.                     for (k=l;k<=n;k++)
  144.                         v[k][j] += s*v[k][i];
  145.                 }
  146.             }
  147.             for (j=l;j<=n;j++)
  148.                 v[i][j]=v[j][i]=0.0;
  149.         }
  150.         v[i][i] = 1.0;
  151.         g = rv1[i];
  152.         l = i;
  153.     }
  154.     for (i=n;i>=1;i--) {
  155.         l = i+1;
  156.         g = w[i];
  157.         if (i < n)
  158.             for (j=l;j<=n;j++)
  159.                 a[i][j] = 0.0;
  160.         if (g) {
  161.             g = 1.0/g;
  162.             if (i != n) {
  163.                 for (j=l;j<=n;j++) {
  164.                     for (s=0.0,k=l;k<=m;k++)
  165.                         s += a[k][i]*a[k][j];
  166.                     f = (s/a[i][i])*g;
  167.                     for (k=i;k<=m;k++)
  168.                         a[k][j] += f*a[k][i];
  169.                 }
  170.             }
  171.             for (j=i;j<=m;j++)
  172.                 a[j][i] *= g;
  173.         } else {
  174.             for (j=i;j<=m;j++)
  175.                 a[j][i]=0.0;
  176.         }
  177.         ++a[i][i];
  178.     }
  179.     for (k=n;k>=1;k--) {
  180.         for (its=1;its<=30;its++) {
  181.             flag = 1;
  182.             for (l=k;l>=1;l--) {
  183.                 nm = l-1;
  184.                 if (fabs(rv1[l])+anorm == anorm) {
  185.                     flag=0;
  186.                     break;
  187.                 }
  188.                 if (fabs(w[nm])+anorm == anorm) break;
  189.             }
  190.             if (flag) {
  191.                 c = 0.0;
  192.                 s = 1.0;
  193.                 for (i=l;i<=k;i++) {
  194.                     f=s*rv1[i];
  195.                     if (fabs(f)+anorm != anorm) {
  196.                         g = w[i];
  197.                         h = PYTHAG(f,g);
  198.                         w[i]=h;
  199.                         h=1.0/h;
  200.                         c=g*h;
  201.                         s=(-f*h);
  202.                         for (j=1;j<=m;j++) {
  203.                             y=a[j][nm];
  204.                             z=a[j][i];
  205.                             a[j][nm]=y*c+z*s;
  206.                             a[j][i]=z*c-y*s;
  207.                         }
  208.                     }
  209.                 }
  210.             }
  211.             z=w[k];
  212.             if (l == k) {
  213.                 if (z < 0.0) {
  214.                     w[k] = -z;
  215.                     for (j=1;j<=n;j++)
  216.                         v[j][k]=(-v[j][k]);
  217.                 }
  218.                 break;
  219.             }
  220.             if (its == 30) {
  221.                 fprintf(stderr, "svdcmp: No convergence in 30 iterations.\n");
  222.                 return(ERRR);
  223.             }
  224.             x=w[l];
  225.             nm=k-1;
  226.             y=w[nm];
  227.             g=rv1[nm];
  228.             h=rv1[k];
  229.             f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
  230.             g=PYTHAG(f,1.0);
  231.             f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
  232.             c=s=1.0;
  233.             for (j=l;j<=nm;j++) {
  234.                 i=j+1;
  235.                 g=rv1[i];
  236.                 y=w[i];
  237.                 h=s*g;
  238.                 g=c*g;
  239.                 z=PYTHAG(f,h);
  240.                 rv1[j]=z;
  241.                 c=f/z;
  242.                 s=h/z;
  243.                 f=x*c+g*s;
  244.                 g=g*c-x*s;
  245.                 h=y*s;
  246.                 y=y*c;
  247.                 for (jj=1;jj<=n;jj++) {
  248.                     x=v[jj][j];
  249.                     z=v[jj][i];
  250.                     v[jj][j]=x*c+z*s;
  251.                     v[jj][i]=z*c-x*s;
  252.                 }
  253.                 z=PYTHAG(f,h);
  254.                 w[j]=z;
  255.                 if (z) {
  256.                     z=1.0/z;
  257.                     c=f*z;
  258.                     s=h*z;
  259.                 }
  260.                 f=(c*g)+(s*y);
  261.                 x=(c*y)-(s*g);
  262.                 for (jj=1;jj<=m;jj++) {
  263.                     y=a[jj][j];
  264.                     z=a[jj][i];
  265.                     a[jj][j]=y*c+z*s;
  266.                     a[jj][i]=z*c-y*s;
  267.                 }
  268.             }
  269.             rv1[l]=0.0;
  270.             rv1[k]=f;
  271.             w[k]=x;
  272.         }
  273.     }
  274.     return(1);
  275. }
  276.  
  277. #undef SIGN
  278. #undef MAX
  279. #undef PYTHAG
  280.  
  281. static int svbksb(double **u, double *w, double **v, int m, int n, double *b, double *x)
  282. {
  283.     int jj, j, i;
  284.     double s, *tmp;
  285.  
  286.     ADVECTOR(tmp, 1, n, "svbksb", return);
  287.  
  288.     for (j=1;j<=n;j++) {
  289.         s=0.0;
  290.         if (w[j]) {
  291.             for (i=1;i<=m;i++) s += u[i][j]*b[i];
  292.             s /= w[j];
  293.         }
  294.         tmp[j]=s;
  295.     }
  296.     for (j=1;j<=n;j++) {
  297.         s=0.0;
  298.         for (jj=1;jj<=n;jj++) s += v[j][jj]*tmp[jj];
  299.         x[j]=s;
  300.     }
  301.     return(1);
  302. }
  303.  
  304. int Ft_svdvar(double **v, int ma, double *w, double **cvm)
  305. {
  306.     int k,j,i;
  307.     double sum, *wti;
  308.  
  309.     ADVECTOR(wti, 1, ma, "svdvar", Ft_catcher);
  310.  
  311.     for (i=1;i<=ma;i++) {
  312.         wti[i]=0.0;
  313.         if (w[i]) wti[i]=1.0/(w[i]*w[i]);
  314.     }
  315.     for (i=1;i<=ma;i++) {
  316.         for (j=1;j<=i;j++) {
  317.             for (sum=0.0,k=1;k<=ma;k++) sum += v[i][k]*v[j][k]*wti[k];
  318.             cvm[j][i]=cvm[i][j]=sum;
  319.         }
  320.     }
  321.     return(0);
  322. }
  323.